#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#可以实现保持一个固定值后重置为0，并且从0开始测力
"""
    力传感器信息采集
    2023/10/31
    maobinghao
"""
import rospy
import serial
import serial.tools.list_ports
import time
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Header

SERIAL_IS_OPEN = False  # 默认串口关闭
SERIAL_IS_GET = False  # 默认串口关闭
port_name_list = []  # 端口名称列表
port_com_list = []  # 端口号列表
MySerial = None  # 打开的串口

getFz = 0

hex_data1 = bytes([0xFE, 0x04, 0x00, 0x00, 0x00, 0x01, 0x25, 0xC5])
hex_data2 = bytes([0xFE, 0x04, 0x00, 0x01, 0x00, 0x01, 0x74, 0x05])
hex_data3 = bytes([0xFE, 0x04, 0x00, 0x02, 0x00, 0x01, 0x84, 0x05])
hex_datas = (hex_data1, hex_data2, hex_data3)

# 稳定性判定参数
stability_threshold = 5  # 稳定性阈值
stability_count = 5     # 稳定计数阈值
stable_readings_x = []   # 用于判断x方向是否稳定
stable_readings_y = []   # 用于判断y方向是否稳定
stable_readings_z = []   # 用于判断z方向是否稳定

# 初始校准值
calibration_offset_x = 0
calibration_offset_y = 0
calibration_offset_z = 0

# 扫描可用的串口设备
def scan_serial():
    port_list = list(serial.tools.list_ports.comports())
    if len(port_list) > 0:
        global SERIAL_IS_GET
        SERIAL_IS_GET = True
        print(port_list)
        return port_list
    else:
        return []

def send_data(ser, data):
    ser.write(data)

def reset_and_calibrate(stable_readings, force, calibration_offset):
    """
    判断是否稳定在一个值，如果是则将其置为0，并更新校准值
    """
    stable_readings.append(force)
    if len(stable_readings) > stability_count:
        stable_readings.pop(0)
        if max(stable_readings) - min(stable_readings) < stability_threshold:
            calibration_offset += force  # 更新校准值
            return 0, calibration_offset  # 重置为0并更新校准
    return force, calibration_offset

if __name__ == "__main__":
    rospy.init_node("force_three")
    serial_list = scan_serial()
    SERIAL_IS_OPEN = True
    rate = rospy.Rate(50)
    while SERIAL_IS_OPEN:
        serial_list = scan_serial()
        if len(port_name_list) != len(serial_list):
            port_name_list.clear()
            port_com_list.clear()
            for i in range(len(serial_list)):
                port_name_list.append(serial_list[i].description)
                port_com_list.append(serial_list[i].device)

            # print("Serial devices found:", port_com_list[0])
            SERIAL_IS_OPEN = False
        else:
            print("未扫描到串口设备")
    # print("Serial devices found:", serial_list)

    ser = serial.Serial(port_com_list[0], 9600, timeout=0.1)
    ser.flushInput()
    ser.flushOutput()
    print("串口打开成功!!!")

    twist_force = TwistStamped()
    pub_sensor_readings = rospy.Publisher('/force_data', TwistStamped, queue_size=100)

    try:
        while True:
            try:
                if not ser:
                    ser = serial.Serial(port_com_list[0], 9600, timeout=0.1)
                    ser.flushInput()
                    ser.flushOutput()
                for i in range(3):
                    if ser.isOpen():
                        ser.write(hex_datas[i])
                        time.sleep(0.01)
                    else:
                        raise ValueError('Failed to open serial port')
                    data = ser.readline()
                    if data:
                        if i == 0:
                            getFx = (data[3] << 8 | data[4])
                            if getFx > 50000:
                                getFx = getFx - 65535
                            getFx -= calibration_offset_x
                            getFx, calibration_offset_x = reset_and_calibrate(stable_readings_x, getFx, calibration_offset_x)
                            twist_force.twist.linear.x = getFx / 100
                        if i == 1:
                            getFy = (data[3] << 8 | data[4])
                            if getFy > 50000:
                                getFy = getFy - 65535
                            getFy -= calibration_offset_y
                            getFy, calibration_offset_y = reset_and_calibrate(stable_readings_y, getFy, calibration_offset_y)
                            twist_force.twist.linear.y = getFy / 100
                        if i == 2:
                            getFz = (data[3] << 8 | data[4])
                            if getFz > 50000:
                                getFz = getFz - 65535
                            getFz -= calibration_offset_z
                            getFz, calibration_offset_z = reset_and_calibrate(stable_readings_z, getFz, calibration_offset_z)
                            twist_force.twist.linear.z = getFz / 100
                        twist_force.header = Header(stamp=rospy.Time.now(), frame_id='base_link')
                        pub_sensor_readings.publish(twist_force)
            except serial.SerialException:
                print("采集数据出错")
            rate.sleep()
    except KeyboardInterrupt:
        print("程序已中断")
        if ser and ser.isOpen():
            ser.close()
        print("串口已关闭")